/*****************************************************
  Mars explorer NXT code in Ch
  University of Leicester GReP 2011
  Using http://iel.ucdavis.edu/projects/chmindstorms/
 *****************************************************/
/*****************************************************
  recordultra.ch 
  
  Used to test the ultrasound sensor setup by scanning
  the surrounding area from a stationary position
  and plotting a graph of distance against angle.
 *****************************************************/

#include <conio.h>
#include <stdio.h>
#include <iostream.h>

#include "control.h"

int main()
{
  int i;
  int numpoints = 40;
	double angle[numpoints]; //this stores the actual angle moved as reported from the tachometer
	double distance[numpoints]; //this stores the data received from the ultrasonic sensor

  FILE *output;
  setup();
  Sensor ultra = Sensor(PORT_4, ULTRASONIC, RAWMODE);
  Sensor compass = Sensor(PORT_3, ULTRASONIC, RAWMODE);
  
  output = fopen("output/ultra.dat", "w");
  
  scanUltra(ultra, compass, numpoints, angle, distance);
  
  for (i=0; i<numpoints; i++) // write ultrasound data to file
    fprintf(output, "%f %f\n", angle[i], distance[i]);
    
  fclose(output);
  system("wgnuplot.exe output\\ultra.plt"); // plot readings
  
	return 0;
}
